#include "App_FreeRtos.h"
uint16_t height = 0;
uint16_t current_height = 0;
EulerAngle_Struct euler_angle = {0};
Remote_struct remote = {0};
GyroAccel_Struct gyro_accel = {0};
// 定义左上电机结构体
Motor_struct left_top_motor = {.htim = &htim3, .channel = TIM_CHANNEL_1, .speed = 0};
// 定义左下电机结构体
Motor_struct left_bottom_motor = {.htim = &htim4, .channel = TIM_CHANNEL_4, .speed = 0};
// 定义右上电机结构体
Motor_struct right_top_motor = {.htim = &htim2, .channel = TIM_CHANNEL_2, .speed = 0};
// 定义右下电机结构体
Motor_struct right_bottom_motor = {.htim = &htim1, .channel = TIM_CHANNEL_3, .speed = 0};
// 定义电机周期
#define MOTOR_PERIOD 8

// 定义左上角LED结构体
LED_struct left_top_led = {.port = LED1_GPIO_Port, .pin = LED1_Pin};
LED_struct left_bottom_led = {.port = LED3_GPIO_Port, .pin = LED3_Pin};
LED_struct right_top_led = {.port = LED2_GPIO_Port, .pin = LED2_Pin};
LED_struct right_bottom_led = {.port = LED4_GPIO_Port, .pin = LED4_Pin};

Control_status control_status = CONNECT;
Fly_status fly_status = IDLE;
// 定义电源任务
void Power_Task(void *pvParameters);
#define POWER_TASK_PRIORITY 2
#define POWER_TASK_STACK_SIZE 128
TaskHandle_t Power_Task_Handle;
#define LED_PERIOD 50
// 定义LED任务
void LEDTask(void *pvParameters);
#define LED_TASK_PRIORITY 2
#define LED_TASK_STACK_SIZE 128
TaskHandle_t LED_Task_Handle;
// 定义电机任务
void Motor_Task(void *pvParameters);
#define MOTOR_TASK_PRIORITY 3
#define MOTOR_TASK_STACK_SIZE 128
TaskHandle_t Motor_Task_Handle;
// 定义通信任务
void Communication_Task(void *pvParameters);
#define COMMUNICATION_TASK_PRIORITY 2
#define COMMUNICATION_TASK_STACK_SIZE 128
TaskHandle_t Communication_Task_Handle;
#define Communication_PERIOD 6
// 开启FreeRTOS
void App_FreeRtos_Start(void)
{

    // 创建任务
    xTaskCreate(Power_Task, "Power_Task", POWER_TASK_STACK_SIZE, NULL, POWER_TASK_PRIORITY, &Power_Task_Handle);
    xTaskCreate(LEDTask, "LEDTask", LED_TASK_STACK_SIZE, NULL, LED_TASK_PRIORITY, &LED_Task_Handle);
    xTaskCreate(Motor_Task, "Motor_Task", MOTOR_TASK_STACK_SIZE, NULL, MOTOR_TASK_PRIORITY, &Motor_Task_Handle);
    xTaskCreate(Communication_Task, "Communication_Task", COMMUNICATION_TASK_STACK_SIZE, NULL, COMMUNICATION_TASK_PRIORITY, &Communication_Task_Handle);
    // 启动调度器
    vTaskStartScheduler();
}
void Power_Task(void *pvParameters)
{
    // 获取当前时间
    uint32_t current_time = xTaskGetTickCount();
    while (1)
    {
        // vTaskDelayUntil(&current_time, 10000);
        // 等待通知
        uint32_t value = ulTaskNotifyTake(pdTRUE, 10000);
        if (value == 1)
        {
            // 关闭电源
            Int_IP5305T_STOP();
        }
        else if (value == 0)
        {
            Int_IP5305T_Start();
        }
    }
}
void LEDTask(void *pvParameters)
{
    uint32_t current_time = xTaskGetTickCount();
    while (1)
    {
        switch (control_status)
        {
        case CONNECT:
            Int_LED_TurnOn(&left_top_led);
            Int_LED_TurnOn(&right_top_led);
            break;
        case DISCONNECT:
            Int_LED_TurnOff(&left_top_led);
            Int_LED_TurnOff(&right_top_led);
            break;
        default:
            break;
        }
        switch (fly_status)
        {
        case IDLE:
            Int_LED_Toggle(&left_bottom_led);
            Int_LED_Toggle(&right_bottom_led);
            vTaskDelayUntil(&current_time, 500 - LED_PERIOD);
            break;
        case NORMAL:
            Int_LED_Toggle(&left_bottom_led);
            Int_LED_Toggle(&right_bottom_led);
            vTaskDelayUntil(&current_time, 100 - LED_PERIOD);
            break;
        case FIX_HEIGHT:
            Int_LED_TurnOn(&left_bottom_led);
            Int_LED_TurnOn(&right_bottom_led);
            break;
        case FAULT:
            Int_LED_TurnOff(&left_bottom_led);
            Int_LED_TurnOff(&right_bottom_led);
            break;
        default:
            break;
        }
        vTaskDelayUntil(&current_time, LED_PERIOD);
    }
}

void Motor_Task(void *pvParameters)
{
    uint8_t count = 0;
    // 初始化电机
    Int_Motor_Init();
    Int_MPU6050_Init();
    Int_VL53L1X_Init();
    // 获取当前时间
    uint32_t current_time = xTaskGetTickCount();
    while (1)
    {
        // 获取欧拉角,并滤波
        App_Flight_Get_Euler_Angle(&gyro_accel, &euler_angle);
        // PID控制
        App_Flight_PIDControl(&euler_angle, &gyro_accel, &remote);

        current_height = Int_VL53L1X_GetDistance();

        if (fly_status == FIX_HEIGHT)
        {
            count++;
            if (count >=3)
            {
                // 高度控制
                App_Flight_Height_PIDControl(height, current_height, 0.08);
                count = 0;
            }
        }

        // 设置电机速度
        App_Flight_SetSpeed(fly_status, &remote);
        // 延时
        // COM_DEBUG_PRINTFln(":%d", distance);
        vTaskDelayUntil(&current_time, MOTOR_PERIOD);
    }
}
void Communication_Task(void *pvParameters)
{

    // 获取当前系统时间
    uint32_t current_time = xTaskGetTickCount();
    // 初始化SI24R1
    Int_SI24R1_Init();
    while (1)
    {
        // 获取通信状态
        Com_State com_state = App_Communication_Data(&remote);
        // 根据通信状态进行控制
        App_Communication_Control(&control_status, com_state);
        // 如果通信状态正常
        if (com_state == COM_OK)
        {
            // 如果远程控制关机
            if (remote.SHUT_DOWN == 1)
            {
                // 通知Power_Task任务
                xTaskNotifyGive(Power_Task_Handle);
            }
            // 如果远程控制MPU6050复位
            if (remote.MPU6050_RESET == 1)
            {
                // 执行MPU6050校准
                Int_MPU6050_Calibration();
            }
        }
        // 根据控制状态进行飞行
        App_Communication_Fly(&fly_status, control_status, &remote);
        // 延时，直到下一次通信周期
        vTaskDelayUntil(&current_time, Communication_PERIOD);
    }
}
